#ifndef KERNELHMC_HPP
#define KERNELHMC_HPP
using namespace arma; 

template<class Density, class Proposal>//Proposal avec r et d
class KernelHMC{
	public:
		KernelHMC(Density *d, Proposal *p, mat S)
		{
			_d=d;
			//_p=malloc(sizeof(Proposal));
			_p=p;
			boost::random::uniform_01<> U;
			runif=new RandomG::Random<boost::random::uniform_01<> >(U);
			_Sig=1;
			_step=1;
			vec eigval;
			_delta=100;
			mat eigvec;
			eig_sym(eigval, eigvec, S);
			eps=0.0000000001;//4*sqrt(as_scalar(max(eigval)));
			cout << "eps " << eps;
			_M=inv(S);
						
		}
		~KernelHMC(){
			delete runif;
		}
		KernelHMC<Density,Proposal>(const KernelHMC<Density,Proposal>& X ){
			cout << "Hello";
		}
	
/*		KernelHMC<Density,Proposal>& KernelHMC<Density,Proposal>::operator=(const KernelHMC<Density,Proposal>& X){
			cout << "Hello";	
		}
*/
		void testMove(mat *X, int i)
		{
			Move(X,1);
		}
		void Move(mat *X,int _n)//Move matrix convert to multidimentional theta 
		{		//, moves each column dimentionis in line
			int n=X->n_rows;//Y and X as pointers
			int m=X->n_cols;
			if(_bool==1)
			{
				Moved.zeros();
			}
			mat Y(n,m);
			Y.fill(0);
			mat xt;
			_p->Set_n(m);
			_sum=0;
			_i=0;
			for(int i=0;i<n;i++)
			{
				Y(i,span::all)=Prop((*X)(i,span::all));//Y[i]=p->r(X[i])
				mat xt=Y.row(i);
				xt=Accept(X->row(i),Y.row(i),_n);
				(*X)(i,span::all)=xt;			
				_i++;
			}
			_accept=(double)_sum/n;

			cout << "accept = " << (double)_sum/n;

			//////////
			//delete[] y;
		}
		mat Accept(mat X, mat Y,int n){
	
			mat res=X;
			int d=X.n_elem;
			//cout << "Y1 " <<_Y1;
			if(_bool==1)
			{
				double rho=0;
				double temp=_d->Density_utn(Y.t(),n)-(0.5)*log(pow(2*PI,d)*det(_M))-0.5*as_scalar(_Yt*inv(_M)*_Yt.t());//-H(thetastar)
				rho=(temp)-(_d->Density_utn(X.t(),n)-(0.5)*log(pow(2*PI,d)*det(_M))-0.5*as_scalar(_Y1*inv(_M)*_Y1.t()));//-H(thetastar)--H(theta)
				double u=log((*runif)());
				//cout << exp(rho) << "\n";
				if(u<rho)
				{
					Dens(_i,0)=temp;
					res=Y;
					_sum+=1;
					Moved(_i,0)=1;
				}else{
					Dens(_i,0)=temp;
				}
			}else{
				
				double rho=0;
				double temp=_d->Density_utn(Y.t(),n)-(0.5)*log(pow(2*PI,d)*det(_M))-0.5*as_scalar(_Yt*inv(_M)*_Yt.t());
				rho=temp-(Dens(_i,0));
				double u=log((*runif)());
				if(u<rho)
				{
					res=Y;
					_sum+=1;
					Dens(_i,0)=temp;
					Moved(_i,0)=1;
				}

			}
			return res;
		}
		mat Prop(mat X)
		{
			mat Y;
			mat Yt;
			mat Y1;
			mat Xt;
			
			_Y1=(_p->r(1)).t();
			for(int j=0;j<_step;j++)
			{
			//	cout << X;
				if(j==0){Y1=_Y1;}
				Y=Y1+0.5*eps*(_d->Grad(X.t())).t();
				//cout << "Y " << Y;
				//cout << (_d->Grad(X.t()));
				Xt=X+eps*Y*inv(_M);
				Yt=Y+0.5*eps*(_d->Grad(Xt.t())).t();
				X=Xt;
				Y1=Yt;
				_Yt=Yt;
				//cout << j;
			}
				//cout << Yt;
			return Xt; 
		}
		void Set_s(mat X){
			/*
			try{
				//mat r=chol(s);
				_p->Set_s((1/_Sig)*s);
				_M=(1/_Sig)*s;	
			//	cout << "M " <<_M;
				//_p->Set_s(_Sig*_p->Get_s());
			}catch(std::exception& e)
			{
				throw e;
			}*/
			
			mat s1=_p->Get_s();
			mat s=0.5*(cov(X)+s1);
			eps*=_Sig;
			_p->Set_s(s);
			//_step=(int)(_step*_Sig);
			//cout << "nb step " << _step;
		}
		//void Set_nstep(int fact){_nstep=(int)(_nstep*fact);}
		double Get_accept(void){return _accept;	}
	 	void Set_Sigma(double x){_Sig=_Sig*x;}
		void Set_bool(int b){_bool=b;}
		void Set_n(int b){
			_n=b;
			mat De(_n,1);
			mat M(_n,1);
			Moved=M;
			Dens=De;
		}
		double nMove(void)
		{
			return as_scalar(mean(Moved,0));
		}
	private:
		int _sum;
		Density *_d;
		Proposal *_p;
		int _bool;
		double _delta;
		RandomG::Random<boost::random::uniform_01<> > *runif;
		mat Dens; 
		int _n;
		double _accept;
		double _Sig;
		int _i;
		double eps;
		int _step;
		mat Moved;
		mat _M;//mass matrix
		mat _Y1;
		mat _Yt;
};
#endif

